/*
 * Calibrate.cpp
 *
 *  Created on: 03.12.2012
 *      Author: Michael
 */

#include "Calibrate.h"
#include "stdlib.h"

#include "../hardware.h"

extern Magnetometer mag;
extern Gyroscope gyro;
extern Systemstate state;

Calibrate::Calibrate(const char* name, const long prio) :
		Thread(name, prio, 2000) {
	this->gyroBias = 0;
	this->magVar = 0;
}

Calibrate::~Calibrate() {
}

void Calibrate::run() {

	setPeriodicBeat(0 * MILLISECONDS, SAMPLETIME_CAL_MS * MILLISECONDS);
	suspendUntilNextBeat();
	int samples = 0;

	float gyroSum = 0;
	float magSum = 0;
//	xprintf("i'm here %d\n", samples);

	float *mag_calib = (float*) malloc(sizeof(float) * BIAS_SAMPLE_CNT);

	while (samples < BIAS_SAMPLE_CNT) {

		// Gyro
		gyroSum += gyro.getScaledAngularRateZ();
		// Magnetometer
		mag_calib[samples] = mag.getOrientation();
		magSum += mag_calib[samples];

		samples++;
		xprintf("#%d\n", samples);
		suspendUntilNextBeat();
	}

	// Gyro
	gyroBias = gyroSum / (float) BIAS_SAMPLE_CNT;

	// Magnetometer
	float magMean = magSum / BIAS_SAMPLE_CNT;
	for (int i = 0; i < BIAS_SAMPLE_CNT; i++) {
		magVar += (mag_calib[i] - magMean) * (mag_calib[i] - magMean);
	}
	magVar = magVar / BIAS_SAMPLE_CNT;

	state.gyroBias = gyroBias;
	ORANGE_ON;
//	state.magVar = magVar;
//	xprintf("%f , %f\n", gyroBias, magVar);

}
